package com.grt192.benchtest.mechanism;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Mechanism;

/**
 *
 * @author anand
 */
public class BenchDriveTrain extends Mechanism {

    private GRTJaguar frontLeft;
    private GRTJaguar frontRight;
    private GRTJaguar backLeft;
    private GRTJaguar backRight;
    private boolean debug;

    public BenchDriveTrain(int fl, int fr, int bl, int br, boolean debug) {
        frontLeft = new GRTJaguar(fl);
        frontLeft.start();
        frontRight = new GRTJaguar(fr);
        frontRight.start();
        backLeft = new GRTJaguar(bl);
        backLeft.start();
        backRight = new GRTJaguar(br);
        backRight.start();
        addActuator("backleft", backLeft);
        addActuator("backright", backRight);
        addActuator("frontleft", frontLeft);
        addActuator("frontright", frontRight);
        this.debug = debug;
    }

    public void stop() {
        tankDrive(0, 0);
    }

    public void spinRight() {
        spinRight(1.0);
    }

    public void spinRight(double speed) {
        tankDrive(speed, -speed);
    }

    public void spinLeft() {
        spinLeft(1.0);
    }

    public void spinLeft(double speed) {
        tankDrive(-speed, speed);
    }

    public void driveForward() {
        driveForward(1.0);
    }

    public void driveForward(double speed) {
        tankDrive(speed, speed);
    }

    public void driveBackward() {
        driveBackward(1.0);
    }

    public void driveBackward(double speed) {
        tankDrive(-speed, -speed);
    }

    public void carDrive(double x, double y, boolean spin) {
        if (spin) {
            tankDrive(-y, y);
        } else {
            double leftSpeed = x;
            double rightSpeed = x;
            if (y < 0) {
                leftSpeed *= (1.0 + y);
            } else if (y > 0) {
                rightSpeed *= (1.0 - y);
            }
            if (debug) {
                System.out.println(">> " + x + " " + y);
            }
            tankDrive(leftSpeed, rightSpeed);
        }
    }

    public void tankDrive(double leftSpeed, double rightSpeed) {
        if (debug) {
            System.out.println("< " + leftSpeed + " > " + rightSpeed);
        }

        frontLeft.enqueueCommand(-leftSpeed);
        backLeft.enqueueCommand(-leftSpeed);

        frontRight.enqueueCommand(-rightSpeed);
        backRight.enqueueCommand(-rightSpeed);
    }
}
